﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using IMIForUnity;
using UnityEngine;

namespace IMIForUnity
{
    public class BackToHome : ImiGesture
    {
        private float BackToHome_leftForearmTh_min = 35.0f;
        private float BackToHome_leftForearmTh_max = 60.0f;

        public BackToHome()
        {
            name = "BackToHome";
            startTime = 0;
            state = ImiGesture.State.UNCHECKED;
        }

        //public BackToHome(float BackToHome_leftForearmTh_min, float BackToHome_leftForearmTh_max)
        //{
        //    name = "BackToHome";
        //    state = ImiGesture.State.UNCHECKED;
        //    this.BackToHome_leftForearmTh_max = BackToHome_leftForearmTh_max;
        //    this.BackToHome_leftForearmTh_min = BackToHome_leftForearmTh_min;
        //}

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (checkCondition(playerInfo))
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime <= 2.0f)
                    {
                        if (checkCondition(playerInfo))
                        {
                            return State.CHECKING;
                        }else
                        {
                            return State.CANCELED;
                        }
                    }
                    else
                    {
                        state = State.FINISHED;
                    }
                    break;
            }
            return state;
        }

        bool checkCondition(ImiPlayerInfo playerInfo)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] limbsAngular = playerInfo.GetPlayerLimbsAngular();
            bool result = jointsTracked[leftHandIndex] && jointsTracked[leftElbowIndex] &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightElbowIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftElbowIndex].y) < 0 &&
                        (jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) > -0.17f &&
                        (jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) < 0.1f &&
                        limbsAngular[leftForearmIndex].x >= BackToHome_leftForearmTh_min && limbsAngular[leftForearmIndex].x <= BackToHome_leftForearmTh_max &&
                        (jointsPos[rightElbowIndex].y - jointsPos[rightShoulderIndex].y) < 0 &&
                        Math.Abs(limbsAngular[rightForearmIndex].x) >= 60.0f && Math.Abs(limbsAngular[rightForearmIndex].x) <= 90.0f &&
                        Math.Abs(limbsAngular[rightPostbrachiumIndex].x) >= 60.0f && Math.Abs(limbsAngular[rightPostbrachiumIndex].x) <= 90.0f;

            return result;
        }
    }



    public class Calibration : ImiGesture
    {

        private float Calibration_horizontalForearmTh = 20.0f;//15.0f;
        private float Calibration_horizontalDif = 40.0f;//30.0f;

        public Calibration()
        {
            name = "Calibration";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (checkCondition(playerInfo))
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                        
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime <= 2.0f)
                    {
                        if (checkCondition(playerInfo))
                        {
                            return State.CHECKING;
                        }else
                        {
                            return State.CANCELED;
                        }

                    }
                    else
                    {
                        state = State.FINISHED;
                    }
                    break;
            }
            return state;
        }

        private bool checkCondition(ImiPlayerInfo playerInfo)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] limbsAngular = playerInfo.GetPlayerLimbsAngular();
            bool result = jointsTracked[leftHandIndex] && jointsTracked[leftElbowIndex] && jointsTracked[leftShoulderIndex] &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightElbowIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[leftHandIndex].x - jointsPos[leftElbowIndex].x) < 0 &&
                        (jointsPos[rightHandIndex].x - jointsPos[rightElbowIndex].x) > 0 &&
                        Math.Abs(jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) < 0.3f &&
                        Math.Abs(jointsPos[rightHandIndex].z - jointsPos[rightElbowIndex].z) < 0.3f &&
                        Math.Abs(limbsAngular[leftForearmIndex].x) < Calibration_horizontalForearmTh && Math.Abs(limbsAngular[leftForearmIndex].x - limbsAngular[leftPostbrachiumIndex].x) < Calibration_horizontalDif &&
                        Math.Abs(limbsAngular[rightForearmIndex].x) < Calibration_horizontalForearmTh && Math.Abs(limbsAngular[rightForearmIndex].x - limbsAngular[rightPostbrachiumIndex].x) < Calibration_horizontalDif;

            return result;
        }

        /// <summary>
        /// 双手平举的姿势，是默认的获取主玩家的姿势。
        /// </summary>
        /// <param name="gesture"></param>
        /// <param name="player"></param>
        /// <param name="gevent"></param>
        public override void GestureEvent(ImiGesture gesture, ImiPlayerInfo player, GestureEvent gevent)
        {
            if (gevent == IMIForUnity.GestureEvent.Complete)
            {
                ImiManager imiManager = ImiManager.GetInstance();
                if (imiManager.GetPlayerControlMode() != PlayerControlMode.GESTURE)
                {
                    return;
                }

                int newMainPlayerID = player.GetUserId();
                ImiManagerImp.GetInstance().setControlPlayerIdWithoutModeChanging(newMainPlayerID);
            }
        }
    }





    public class RaiseRightHand : ImiGesture
    {
        private static float RaiseHand_Y = 0.2f;


        public RaiseRightHand()
        {
            name = "RaiseRightHand";
            startTime = 0f;
            state = State.UNCHECKED;
        }
        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {

            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();
            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightShoulderIndex].y) > RaiseHand_Y)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;
                case State.CHECKING:

                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 1.0f && deltaTime <= 3.0f)
                    {
                        if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                            (jointsPos[rightHandIndex].y - jointsPos[rightShoulderIndex].y) > RaiseHand_Y)
                        {
                            return State.FINISHED;
                        }
                    }
                    else if (deltaTime > 3.0f)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }
            return state;
        }
    }

    public class RaiseLeftHand : ImiGesture
    {
        private static float RaiseHand_Y = 0.2f;

        public RaiseLeftHand()
        {
            name = "RaiseLeftHand";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftShoulderIndex].y) > RaiseHand_Y)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 1.0f && deltaTime <= 3.0f)
                    {
                        if (jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex] &&
                            (jointsPos[leftHandIndex].y - jointsPos[leftShoulderIndex].y) > RaiseHand_Y)
                        {
                            return State.FINISHED;
                        }
                    }
                    else if (deltaTime > 3.0f)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }
            return state;
        }
    }


    public class RightHandWave : ImiGesture
    {
        private bool stepOne = false;
        private bool stepTwo = false;

        private int joint = -1;
        private Vector3 jointPos;


        public RightHandWave()
        {
            name = "RightHandWave";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            //Debug.Log("Checking Right Hand Wave");
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    //Debug.Log("Unchecked  " + jointsTracked[rightHandIndex]+" " + jointsTracked[hipCenterIndex] +" "+ jointsPos[rightHandIndex].y +" "+ jointsPos[hipCenterIndex].y +" "+ jointsSpeed[rightHandIndex].x);
                    if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                        jointsPos[rightHandIndex].y > jointsPos[rightShoulderIndex].y &&
                        jointsSpeed[rightHandIndex].x > 0.02f &&
                        Mathf.Abs(jointsSpeed[rightHandIndex].z) < 0.02f)
                    {
                        startTime = timestamp;
                        stepOne = stepTwo = false;
                        state = State.CHECKING;
                    }
                    break;

                case State.CHECKING:
                    //Debug.Log("Checking...");
                    if (!stepOne)
                    {
                        if ((timestamp - startTime) < 0.8f)
                        {
                            if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                                jointsPos[rightHandIndex].y > jointsPos[rightShoulderIndex].y &&
                                jointsSpeed[rightHandIndex].x < -0.02f && 
                                Mathf.Abs(jointsSpeed[rightHandIndex].z) < 0.02f)
                            {
                                stepOne = true;
                                break;
                            }
                        }
                        else
                        {
                            state = State.CANCELED;
                            stepOne = false;
                        }
                    }
                    if (stepOne)
                    {
                        if ((timestamp - startTime) < 1.8f)
                        {
                            if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                                jointsPos[rightHandIndex].y > jointsPos[rightShoulderIndex].y &&
                                jointsSpeed[rightHandIndex].x > 0.02f && 
                                Mathf.Abs(jointsSpeed[rightHandIndex].z) < 0.02f)
                            {
                                stepTwo = true;
                            }

                            if (stepTwo == true)
                            {
                                return State.FINISHED;
                            }
                        }
                        else
                        {
                            state = State.CANCELED;
                            stepOne = false;
                            stepTwo = false;
                        }
                    }
                    break;
            }
            return state;
        }


    }



    public class LeftHandWaveLeft : ImiGesture
    {

        public LeftHandWaveLeft()
        {
            name = "LeftHandWaveLeft";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] &&
                        jointsTracked[hipCenterIndex] &&
                        jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y &&
                        jointsSpeed[leftHandIndex].x < -0.05f &&
                        jointsSpeed[leftHandIndex].y < 0)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 0.1f && deltaTime <= 1.0f)
                    {
                        if (jointsTracked[leftHandIndex] &&
                        jointsTracked[leftShoulderIndex] &&
                        jointsPos[leftHandIndex].x - jointsPos[leftShoulderIndex].x < -0.2f &&
                        jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y
                        && jointsSpeed[leftHandIndex].x < -0.05f && jointsSpeed[leftHandIndex].y < 0)
                        {
                            return State.FINISHED;
                        }
                    }
                    else if (deltaTime > 1.0f)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }
            return state;
        }
    }




    public class LeftHandWaveRight : ImiGesture
    {

        public LeftHandWaveRight()
        {
            name = "LeftHandWaveRight";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[hipCenterIndex] &&
                        jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y && jointsSpeed[leftHandIndex].x > 0.05f)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 0.1f && deltaTime <= 1.0f)
                    {
                        if (jointsTracked[leftHandIndex] && jointsTracked[hipCenterIndex])
                        {
                            if (jointsSpeed[leftHandIndex].x > 0.05f && jointsPos[leftHandIndex].x > (jointsPos[hipCenterIndex].x + 0.05f) && jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y)
                            {
                                return State.FINISHED;
                            }
                        }
                    }
                    else if (deltaTime > 1.0f)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }
            return state;
        }
    }




    public class RightHandWaveLeft : ImiGesture
    {

        public RightHandWaveLeft()
        {
            name = "RightHandWaveLeft";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] && jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && jointsSpeed[rightHandIndex].x < -0.05f)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 0.1f && deltaTime <= 1.0f)
                    {
                        if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex])
                        {
                            if (jointsSpeed[rightHandIndex].x < -0.05f && jointsPos[rightHandIndex].x < (jointsPos[hipCenterIndex].x - 0.05f) && jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y)
                            {
                                return State.FINISHED;
                            }
                        }
                    }
                    else if (deltaTime > 1.0f)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }
            return state;
        }
    }

    public class RightHandWaveRight : ImiGesture
    {

        private int stage = 0;
        private float rightHandx;

        public RightHandWaveRight()
        {
            name = "RightHandWaveRight";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();
            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] &&
                        jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y &&
                        jointsSpeed[rightHandIndex].x > 0.05f &&
                        jointsSpeed[rightHandIndex].y < 0)
                    {
                        stage = 0;
                        startTime = timestamp;
                        state = State.CHECKING;
                        rightHandx = jointsPos[rightHandIndex].x;
                    }
                    else
                    {
                        state = State.CANCELED;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 0.2f && deltaTime <= 1.0f)
                    {
                        if (jointsTracked[rightHandIndex] &&
                         jointsTracked[rightShoulderIndex] &&
                         jointsPos[rightHandIndex].x - jointsPos[rightShoulderIndex].x > 0.2f &&
                        jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y
                         && jointsSpeed[rightHandIndex].x > 0.05f &&
                         jointsSpeed[rightHandIndex].y < 0)
                        {
                            return State.FINISHED;
                        }
                    }
                    else if (deltaTime > 1.0f)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }
            return state;
        }
    }

    public class RightHandPush : ImiGesture
    {
        private float deltaTime = 0.8f;

        public RightHandPush()
        {
            name = "RightHandPush";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public RightHandPush(float deltaTime)
        {
            name = "RightHandPush";
            startTime = 0;
            state = State.UNCHECKED;
            this.deltaTime = deltaTime;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] &&
                        jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y &&
                        jointsSpeed[rightHandIndex].z < -0.1f &&
                        Math.Abs(jointsSpeed[rightHandIndex].x) < 0.01f)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                    }
                    break;
                case State.CHECKING:
                    float dt = (float)(timestamp - startTime);
                    if (dt < deltaTime && jointsTracked[rightHandIndex] &&
                        jointsSpeed[rightHandIndex].z <= -0.1f &&
                        Math.Abs(jointsSpeed[rightHandIndex].x) < 0.01f)
                    {
                        state = State.FINISHED;
                    }
                    else if (dt > deltaTime)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }

            return state;
        }
    }

    public class LeftHandPush : ImiGesture
    {
        private float deltaTime = 0.8f;
        public LeftHandPush()
        {
            name = "LeftHandPush";
            startTime = 0;
            state = State.UNCHECKED;
        }

        public LeftHandPush(float deltaTime)
        {
            name = "LeftHandPush";
            startTime = 0;
            state = State.UNCHECKED;
            this.deltaTime = deltaTime;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] &&
                        jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y &&
                        jointsSpeed[leftHandIndex].z < -0.1f &&
                        Math.Abs(jointsSpeed[leftHandIndex].x) < 0.01f)
                    {
                        startTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                    }
                    break;
                case State.CHECKING:
                    float dt = (float)(timestamp - startTime);
                    if (dt < deltaTime && jointsTracked[leftHandIndex] &&
                        jointsSpeed[leftHandIndex].z <= -0.1f &&
                        Math.Abs(jointsSpeed[leftHandIndex].x) < 0.01f)
                    {
                        state = State.FINISHED;
                    }
                    else if (dt > deltaTime)
                    {
                        state = State.CANCELED;
                    }
                    break;
            }


            return state;
        }
    }

}
